#ifndef SERIAL_ENCODER_HPP
#define SERIAL_ENCODER_HPP

#include <stdint.h>

#define INT14_MAX (0x1FFF)

// 串口数据帧格式
typedef struct ctr_data {
    // 2bytes
    int8_t chassis_x; // 底盘x方向速度，正方向是向前。 -INT8_MAX, INT8_MAX 对应 -1.5m/s , 1.5m/s
    int8_t chassis_y; // 底盘y方向速度，正方向是向左。-INT8_MAX, INT8_MAX 对应 -1.5m/s , 1.5m/s
    
    // 1byte
    int8_t chassis_yaw; // 底盘yaw方向角速度，正方向是向左。 -INT8_MAX, INT8_MAX 对应 -5rad/s , 5rad/s

    // 1byte
    int8_t down_gimbal_delta_pitch; // 下云台pitch轴角度，-INT8_MAX, INT8_MAX 对应 -0.3 rad，0.3 rad
    
    // 2bytes
    int16_t up_gimbal_shoot:1 ; // 上云台发射
    int16_t down_gimbal_shoot:1 ; // 下云台发射
    int16_t down_gimbal_delta_yaw:14 ; // 下云台yaw轴delta角度, -(INT14_MAX), (INT14_MAX) 对应 -1.0 rad, 1.0 rad
    
    // 1byte
    int8_t up_gimbal_delta_yaw ; // 上云台yaw轴delta角度，-INT8_MAX, INT8_MAX 对应 -1.0 rad，1.0 rad
    
    // 1bytes
    int8_t up_gimbal_delta_pitch; // 上云台pitch轴角度，-INT8_MAX, INT8_MAX 对应 -0.3 rad，0.3 rad
}ctr_data;

#define C_DATA_INT16_OFFSET 4

// 待发送的原始数据，从其他线程计算得到的浮点类型云台转角、底盘速度、整数类型的发射信息
typedef struct ctr_data_raw_float {
    float down_gimbal_delta_yaw; // 下云台yaw轴角度
    float down_gimbal_delta_pitch; // 下云台pitch轴角度
    float up_gimbal_delta_yaw; // 上云台yaw轴角度
    float up_gimbal_delta_pitch; // 上云台pitch轴角度
    float chassis_x; // 底盘x方向速度
    float chassis_y; // 底盘y方向速度
    float chassis_yaw; // 底盘yaw方向角速度
    int8_t down_gimbal_shoot; // 下云台发射
    int8_t up_gimbal_shoot; // 上云台发射
}ctr_data_raw_float;

#define LIMIT_INSIDE(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
#define MAP(x, in_min, in_max, out_min, out_max) ((x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min)
#define PI 3.14159265358979323846f

// 将浮点类型的控制数据转换为整数类型的控制数据，输入是指向一个待发送原始数据结构体的指针和输出结构体的指针
// 需要将超过上下限的值饱和到上下限



void encode_ctr_data(const ctr_data_raw_float *input, ctr_data *output);
void decode_ctr_data(const ctr_data *input, ctr_data_raw_float *output);
// void swap_bytes(uint8_t *data, int index1, int index2);

#endif